/*
 * scheduler.c
 *
 *  Created on: 13.01.2013
 *      Author: Michael
 */

/*
 * ======== Project related includes ========
 */
#include "os/scheduler/scheduler.h"
#include "os/scheduler/scheduleAlgorithm.h"
#include "os/scheduler/thread/thread.h"
#include "os/common/util.h"
#include "os/sysprops.h"

#define THREAD_ID_INVALID (SCHEDULER_MAX_THREADS + 1)

static Thread_t gThreads[SCHEDULER_MAX_THREADS];
static uint16_t gCurrThread = 0;
static schedule_algorithm gScheduleAlgorithm;

static uint16_t getNextFreeSlot();

void scheduler_init(Extension_t exts) {
	ATOMIC_START();

	// set the correct scheduling algorithm
	switch (SCHEDULER_ALGORITHM) {
	case 0:
		gScheduleAlgorithm = scheduleAlgo_roundRobin;
		break;
	default:
		break;
	}

	// initialize the extensions
	uint16_t i;
	for (i = 0; i < exts.numOfExts; ++i) {
		scheduler_startThread(exts.extensions[i]);
	}

	ATOMIC_END();
}

uint16_t scheduler_startThread(thread_func tFunc) {
	// create a new thread
	// FIXME first method not executed
	uint16_t nextThread = getNextFreeSlot();
	gThreads[nextThread].id = nextThread;
	gThreads[nextThread].state = THREAD_READY;
	gThreads[nextThread].priority = PRIORITY_LOW;
	gThreads[nextThread].run = tFunc;

	// start thread
	if (setjmp(gThreads[nextThread].context) == 0) {
		return gThreads[nextThread].id;
	} else {
		// set current stack
		// 0x3FF upper end of ram
		_set_SP_register(0x3FF - (gCurrThread * SCHEDULER_STACKSIZE_PER_THREAD));
		ATOMIC_END();
		gThreads[gCurrThread].run();
		scheduler_killThread(gCurrThread);
		return THREAD_ID_INVALID;
	}
}

void scheduler_runNextThread(void) {
	ATOMIC_START();
	uint16_t nextThread = gScheduleAlgorithm(gThreads, gCurrThread);
	switch (gThreads[nextThread].state) {
	case THREAD_RUNNING:
		break; // already running
	case THREAD_READY:
		if (setjmp(gThreads[gCurrThread].context) == 0) { // saved current execution state (setjmp)

			// change state of the current running thread
			if (gThreads[gCurrThread].state == THREAD_RUNNING)
				gThreads[gCurrThread].state = THREAD_READY;

			// start the next thread
			gCurrThread = nextThread;
			gThreads[gCurrThread].state = THREAD_RUNNING;

			// This will jump to scheduler_startThread
			longjmp(gThreads[gCurrThread].context, 1);
		}
		break;
	default:
		break; // no thread to run -> sleep no atomic
	}
	ATOMIC_END();
}

void scheduler_killThread(uint16_t threadId) {
	ATOMIC_START();
	gThreads[threadId].state = THREAD_INVALID;
	scheduler_runNextThread();
	ATOMIC_END();
}

static uint16_t getNextFreeSlot() {
	// FIXME handle error if no slot available
	// find next free thread slot
	uint16_t i;
	for (i = 0; i < SCHEDULER_MAX_THREADS; ++i) {
		if (gThreads[i].state == THREAD_INVALID) {
			return i;
		}
	}

	return 0;
}
